#include "car_control.h"
#include <app_demo_robot_car.h>

extern hi_u32  g_car_speed;

/*gpio control func*/
hi_void gpio_control(hi_io_name gpio, hi_gpio_idx id, hi_gpio_dir  dir,  hi_gpio_value gpio_val, hi_u8 val)
{
  //gpio_control(HI_IO_NAME_GPIO_9, HI_GPIO_IDX_9, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_9_GPIO);
  if(gpio==HI_IO_NAME_GPIO_9){
      gpio=HI_IO_NAME_GPIO_10;
      id = HI_GPIO_IDX_10;
      val= HI_IO_FUNC_GPIO_10_GPIO;
  }else if(gpio==HI_IO_NAME_GPIO_10){
      gpio=HI_IO_NAME_GPIO_9;
      id = HI_GPIO_IDX_9;
      val= HI_IO_FUNC_GPIO_9_GPIO;
  }

    hi_io_set_func(gpio, val);
    hi_gpio_set_dir(id, dir);
    hi_gpio_set_ouput_val(id, gpio_val);
}

/*pwm control func*/
hi_void pwm_control(hi_io_name id, hi_u8 val, hi_pwm_port port, hi_u16 duty)
{

    //HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1, g_car_speed
    if(id==HI_IO_NAME_GPIO_10){
        id = HI_IO_NAME_GPIO_9;
        val= HI_IO_FUNC_GPIO_9_PWM0_OUT;
        port=HI_PWM_PORT_PWM0;  
    }else if(id==HI_IO_NAME_GPIO_9){
        id = HI_IO_NAME_GPIO_10;
        val= HI_IO_FUNC_GPIO_10_PWM1_OUT;
        port=HI_PWM_PORT_PWM1; 
    }

    hi_io_set_func(id, val);
    hi_pwm_init(port);
    hi_pwm_set_clock(PWM_CLK_160M);
    hi_pwm_start(port, duty, PWM_FREQ_FREQUENCY);
}

/*car speed up */
hi_void car_speed_up(hi_void)
{
    g_car_speed = g_car_speed - CAR_SPEED_CHANGE_VALUE;
    correct_car_speed();
    printf("g_car_speed is %d\n",(int)g_car_speed);
}

/*car speed reduction*/
hi_void car_speed_reduction(hi_void)
{
    g_car_speed = g_car_speed + CAR_SPEED_CHANGE_VALUE;
    correct_car_speed();
    printf("g_car_speed is %d\n",(int)g_car_speed);
}





/*correct car speed*/
hi_void correct_car_speed(hi_void)
{
    if (g_car_speed < MAX_SPEED) {
        g_car_speed = MAX_SPEED;
    }

    if (g_car_speed > MIN_SPEED) {
        g_car_speed = MIN_SPEED;
    }
}


void set_speed(hi_s32 speed01,hi_s32 speed910);

hi_s32 speed01;
hi_s32 speed910;
//pid控制车的运动
hi_void car_pid_control(hi_s32 pid_out){
    if(pid_out<500){
        pid_out = pid_out * 50;
    }else{
        pid_out *= 100;
    }
    

    speed01 = g_car_speed*4  + pid_out;
    speed910 = g_car_speed*3-500 - pid_out;

    
    if(speed01 >= MIN_SPEED){
        speed01 = MIN_SPEED;
    }else if(speed01 <MAX_SPEED) {
        speed01 = MAX_SPEED;
    }

    if(speed910 >= MIN_SPEED){
        speed910 = MIN_SPEED;
    }else if(speed910 < MAX_SPEED){
        speed910 = MAX_SPEED;
    }

    pwm_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_PWM3_OUT,HI_PWM_PORT_PWM3,speed01); 
    gpio_control(HI_IO_NAME_GPIO_1, HI_GPIO_IDX_1, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_1_GPIO);

    pwm_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_PWM0_OUT,HI_PWM_PORT_PWM0, speed910); 
    gpio_control(HI_IO_NAME_GPIO_10, HI_GPIO_IDX_10, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_10_GPIO);    
}


void set_speed(hi_s32 speed01,hi_s32 speed910){
    if( speed01 >= 0){
        pwm_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_PWM3_OUT,HI_PWM_PORT_PWM3, speed01 + 100); 
        gpio_control(HI_IO_NAME_GPIO_1, HI_GPIO_IDX_1, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_1_GPIO);
    }else{
        speed01 *= -1;
        pwm_control(HI_IO_NAME_GPIO_1,HI_IO_FUNC_GPIO_1_PWM4_OUT,HI_PWM_PORT_PWM4, speed01 + 100); 
        gpio_control(HI_IO_NAME_GPIO_0, HI_GPIO_IDX_0, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_0_GPIO);
    }

    if(speed910 >=0){
        pwm_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_PWM0_OUT,HI_PWM_PORT_PWM0, speed910+100); 
        gpio_control(HI_IO_NAME_GPIO_10, HI_GPIO_IDX_10, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_10_GPIO);    
    }else{
        speed910 *= -1;
        pwm_control(HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1, speed910+100); 
        gpio_control(HI_IO_NAME_GPIO_9, HI_GPIO_IDX_9, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_9_GPIO);    
    }

    printf("speed01: %d\r\n",speed01);
    printf("speed910: %d\r\n",speed910);
}

/*car go back */
hi_void car_go_back(hi_void)
{
    correct_car_speed();
    gpio_control(HI_IO_NAME_GPIO_0, HI_GPIO_IDX_0, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_0_GPIO);
    pwm_control(HI_IO_NAME_GPIO_1,HI_IO_FUNC_GPIO_1_PWM4_OUT,HI_PWM_PORT_PWM4, g_car_speed);
    gpio_control(HI_IO_NAME_GPIO_9, HI_GPIO_IDX_9, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_9_GPIO);
    pwm_control(HI_IO_NAME_GPIO_10,HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1, g_car_speed);
}


/*car stop */
hi_void car_stop(hi_void)
{
    correct_car_speed();
    pwm_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_PWM3_OUT,HI_PWM_PORT_PWM3, PWM_DUTY_STOP); 
    gpio_control(HI_IO_NAME_GPIO_1, HI_GPIO_IDX_1, HI_GPIO_DIR_OUT, HI_GPIO_VALUE0, HI_IO_FUNC_GPIO_1_GPIO);
    pwm_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_PWM0_OUT,HI_PWM_PORT_PWM0, PWM_DUTY_STOP);
    gpio_control(HI_IO_NAME_GPIO_10, HI_GPIO_IDX_10, HI_GPIO_DIR_OUT, HI_GPIO_VALUE0, HI_IO_FUNC_GPIO_10_GPIO);
}



/*car go forward */
hi_void car_go_forward(hi_void)
{
    correct_car_speed();
    pwm_control(HI_IO_NAME_GPIO_0,HI_IO_FUNC_GPIO_0_PWM3_OUT,HI_PWM_PORT_PWM3, 4000); 
    gpio_control(HI_IO_NAME_GPIO_1, HI_GPIO_IDX_1, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_1_GPIO);
    pwm_control(HI_IO_NAME_GPIO_9,HI_IO_FUNC_GPIO_9_PWM0_OUT,HI_PWM_PORT_PWM0, 2500); 
    gpio_control(HI_IO_NAME_GPIO_10, HI_GPIO_IDX_10, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_10_GPIO);
}

